#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
模拟ROS的roslaunch命令，启动多个模拟进程
"""

import os
import sys
import time
import signal
import argparse
import subprocess
import threading
from pathlib import Path

# 确保脚本目录在路径中
SCRIPT_DIR = Path(__file__).parent.absolute()
sys.path.insert(0, str(SCRIPT_DIR))

# 存储子进程列表
processes = []
# 运行标志
running = True

def signal_handler(sig, frame):
    """
    信号处理函数，用于捕获Ctrl+C
    """
    global running
    print(f"\n收到中断信号 ({sig})，正在停止所有进程...")
    running = False
    stop_all_processes()

def stop_all_processes():
    """
    停止所有子进程
    """
    for proc in processes:
        try:
            if proc.poll() is None:  # 如果进程仍在运行
                print(f"停止进程: {proc.args}")
                proc.terminate()
                # 给进程一些时间来优雅地终止
                time.sleep(0.5)
                if proc.poll() is None:  # 如果进程仍在运行
                    print(f"强制结束进程: {proc.args}")
                    proc.kill()
        except Exception as e:
            print(f"停止进程时出错: {str(e)}")

def launch_node(name, interval=1.0, cpu_load=0.0):
    """
    启动一个模拟的ROS节点（使用test_process.py）
    
    :param name: 节点名称
    :param interval: 输出间隔（秒）
    :param cpu_load: CPU负载模拟（0-1之间）
    :return: 启动的进程对象
    """
    test_process_path = os.path.join(SCRIPT_DIR, "test_process.py")
    
    # 确保test_process.py存在且可执行
    if not os.path.exists(test_process_path):
        print(f"错误: 找不到测试进程脚本 {test_process_path}")
        return None
    
    # 使脚本可执行（Unix系统）
    if sys.platform != "win32":
        os.chmod(test_process_path, 0o755)
    
    # 构建命令
    cmd = [
        sys.executable,  # Python解释器路径
        test_process_path,
        "--name", name,
        "--interval", str(interval),
        "--cpu-load", str(cpu_load)
    ]
    
    # 启动进程
    print(f"启动节点: {name}")
    try:
        process = subprocess.Popen(
            cmd,
            stdout=subprocess.PIPE,
            stderr=subprocess.STDOUT,
            text=True,
            bufsize=1,  # 行缓冲
            universal_newlines=True
        )
        
        # 启动线程来处理进程输出
        threading.Thread(
            target=handle_process_output,
            args=(process, name),
            daemon=True
        ).start()
        
        return process
    except Exception as e:
        print(f"启动节点 {name} 时出错: {str(e)}")
        return None

def handle_process_output(process, name):
    """
    处理进程的输出
    
    :param process: 子进程对象
    :param name: 进程名称
    """
    prefix = f"[{name}] "
    for line in process.stdout:
        print(prefix + line.strip())

def mock_roslaunch(launch_file, wait=True):
    """
    模拟roslaunch命令，根据launch_file启动不同的节点组合
    
    :param launch_file: 启动文件名（不含路径和扩展名）
    :param wait: 是否等待所有进程结束
    """
    global processes
    
    # 设置信号处理器
    signal.signal(signal.SIGINT, signal_handler)
    signal.signal(signal.SIGTERM, signal_handler)
    
    print(f"模拟启动 ROS launch 文件: {launch_file}")
    
    # 根据不同的launch文件启动不同的节点组合
    if launch_file == "navigation":
        # 导航系统通常包含多个组件
        processes.append(launch_node("map_server", interval=2.0, cpu_load=0.1))
        processes.append(launch_node("amcl", interval=0.5, cpu_load=0.2))
        processes.append(launch_node("move_base", interval=0.2, cpu_load=0.3))
        processes.append(launch_node("path_planner", interval=1.0, cpu_load=0.2))
    
    elif launch_file == "camera":
        # 相机系统通常只有少量组件
        processes.append(launch_node("camera_driver", interval=0.1, cpu_load=0.2))
        processes.append(launch_node("image_proc", interval=0.05, cpu_load=0.4))
    
    elif launch_file == "default" or launch_file == "test":
        # 默认启动一个简单的测试节点
        processes.append(launch_node("test_node", interval=1.0, cpu_load=0.1))
    
    else:
        # 未知launch文件，启动一个通用节点
        processes.append(launch_node(f"{launch_file}_node", interval=1.0, cpu_load=0.1))
    
    print(f"已启动 {len(processes)} 个节点")
    
    # 等待所有进程结束
    if wait and processes:
        try:
            while running and any(p.poll() is None for p in processes):
                time.sleep(0.1)
        except KeyboardInterrupt:
            print("\n用户中断，正在停止所有进程...")
            stop_all_processes()
        finally:
            # 确保所有进程都已停止
            stop_all_processes()
            print("所有节点已停止")

def main():
    """
    主函数
    """
    parser = argparse.ArgumentParser(description='模拟ROS的roslaunch命令')
    parser.add_argument('launch_file', help='要启动的launch文件名（不含扩展名）')
    parser.add_argument('--no-wait', action='store_true', help='不等待进程结束')
    
    args = parser.parse_args()
    
    # 运行模拟的roslaunch
    mock_roslaunch(args.launch_file, not args.no_wait)
    
    return 0

if __name__ == '__main__':
    sys.exit(main()) 